"""
FES控制器模块
控制原理：通过输入sEMG特征计算电刺激强度，再生成电刺激曲线参数，并发送到下位机
"""

from collections import deque

import numpy
import numpy as np
import onnxruntime as ort
import numpy as np
from scipy.optimize import root
import threading
import threading
import queue
import time
# from main import logger
# 创建一个logger
import logging

logger2 = logging.getLogger("MyApp")
logger2.setLevel(logging.DEBUG)  # 设置全局日志级别

# 创建控制台 handler 并设置级别为 INFO
console_handler = logging.StreamHandler()
console_handler.setLevel(logging.INFO)

# 设置日志格式
formatter = logging.Formatter("%(message)s")
console_handler.setFormatter(formatter)

# 添加 handler 到 logger
logger2.addHandler(console_handler)

FLE_Threshold = 1880
Grasp_Threshold = 1860


def cubic_function(x: float, a: float, b: float, c: float, d: float) -> float:
    return a * x ** 3 + b * x ** 2 + c * x + d


def inverse_cubic(y: float, a: float, b: float, c: float, d: float, initial_guess: float = 0) -> float:
    # 定义目标函数
    def target_function(x):
        return cubic_function(x, a, b, c, d) - y

    # 使用root函数求解
    solution = root(target_function, initial_guess)
    # 返回解
    return solution.x[0]


# 示例参数
a, b, c, d = 1, -6, 11, -6
y = 0  # 希望求解的y值


# todo example
# 求解反函数



class FES_Config:
    """
    包含FES参数实时配置
    """

    def __init__(self, ch: int = 5, Worktime: int = 2000, Lastingtime: int = 200, HoldOntime: int = 50,
                 ele_Intensity: int = 1000):
        self.Worktime = Worktime
        self.Lastingtime = Lastingtime
        self.HoldOntime = HoldOntime
        self.ele_Intensity = ele_Intensity
        self.Waittingtime = 500
        self.WaveNum = 1
        self.STI_ch = ch

        self.overrall_intensity = self.calculate_currentFES_Intensity(self)

    def set_FES_config(self, fes_config) -> None:
        """
        配置FES参数
        :param fes_config: FES配置
        :return: None
        """
        self.Worktime = fes_config.Worktime
        self.Lastingtime = fes_config.Lastingtime
        self.HoldOntime = fes_config.HoldOntime
        self.ele_Intensity = fes_config.ele_Intensity
        self.Waittingtime = fes_config.Waittingtime
        self.WaveNum = fes_config.WaveNum
        self.overrall_intensity = self.calculate_currentFES_Intensity(self)

    # 电刺激强度控制函数
    @staticmethod
    def calculate_currentFES_Intensity(fes_config) -> int:
        """
        计算电刺激强度
        :return: 电刺激强度
        """
        # cnt = int(fes_config.Lastingtime / fes_config.HoldOntime)
        worktime_ms = int(fes_config.Worktime / 1000)
        duty = worktime_ms / (worktime_ms + fes_config.HoldOntime)
        return int(duty * fes_config.ele_Intensity)


#非线性映射强度映射表
FES_config_map = {
    1: FES_Config(ch=5, Worktime=500, Lastingtime=100, HoldOntime=50, ele_Intensity=200),
    2: FES_Config(ch=5, Worktime=1000, Lastingtime=100, HoldOntime=50, ele_Intensity=500),
    3: FES_Config(ch=5, Worktime=2000, Lastingtime=200, HoldOntime=50, ele_Intensity=500),
    4: FES_Config(ch=5, Worktime=3000, Lastingtime=200, HoldOntime=50, ele_Intensity=500),
    5: FES_Config(ch=5, Worktime=4000, Lastingtime=300, HoldOntime=50, ele_Intensity=500),
    6: FES_Config(ch=5, Worktime=500, Lastingtime=100, HoldOntime=50, ele_Intensity=1000),
    7: FES_Config(ch=5, Worktime=1000, Lastingtime=200, HoldOntime=50, ele_Intensity=1000),
    8: FES_Config(ch=5, Worktime=2000, Lastingtime=200, HoldOntime=50, ele_Intensity=1000),
    9: FES_Config(ch=5, Worktime=3000, Lastingtime=200, HoldOntime=50, ele_Intensity=1000),
    10: FES_Config(ch=5, Worktime=4000, Lastingtime=200, HoldOntime=50, ele_Intensity=2000),
}


class FES_Controler:
    def __init__(self,
                 rms_input_queue: queue.Queue,  # 新增 RMS 输入队列参数
                 fes_output_queue: queue.Queue = None) -> None:
        # 四通道电刺激配置
        self.CH_fes_Config = [FES_Config(i) for i in range(4)]
        self.FES_ch_mode = [0, 0, 0, 0]  #默认关闭状态
        # self.ch_semg_deque = [deque([], maxlen=40) for _ in range(4)] # deque 可能不再需要，直接处理传入的RMS
        self.ch_RMS = [0, 0, 0, 0]  # 通道RMS值

        self.rms_input_queue = rms_input_queue
        self.fes_output_queue = fes_output_queue
        self.stop_event = threading.Event()  # 添加停止事件

        # 0: 手动控制 1: 同侧控制 2: 对侧闭环控制
        self.control_mode = 0

        # --- 对侧闭环投票机制相关 --- 
        self.VOTING_WINDOW_SIZE = 2  # 投票窗口大小
        self.intensity_history1 = deque(maxlen=self.VOTING_WINDOW_SIZE)
        self.intensity_history2 = deque(maxlen=self.VOTING_WINDOW_SIZE)

        self.last_fes_time = 0

        # ---------------------------

        self.Set_FES_ = 100

        """
        控制系统参数
        """
        self.Kp = 0.5
        self.Ki = 0.1
        self.Kd = 0.1
        self.front_feed = 0.2

        self.hand_WristFlexion = 0  # 静息 = 0,屈腕 = 1
        self.hand_Grasp = 0  # 静息 = 0,抓握 = 1

        # 添加PID控制器所需的变量
        self.prev_error = [0] * 4  # 为每个通道维护独立的误差值
        self.integral = [0] * 4

    def hand_classify(self, RMS_feature: np.ndarray, map_in: list = [0, 1], map_out=[2, 3]) -> tuple:
        """
        手动作分类推理
        :param raw_semg:sEMG矩阵
        map_in:输入端口编号
        :return:hand_WristFlexion，hand_Grasp
        接入训练网络，
        """
        # logger.debug("begin_hand_classify")
        
        if FLE_Threshold < RMS_feature[map_in[0]] < 2080:
            self.FES_ch_mode[map_out[0]] = 1
            logger2.debug("WristFlexion")
        else:
            self.FES_ch_mode[map_out[0]] = 0

        if Grasp_Threshold < RMS_feature[map_in[1]] < 2000\
                and RMS_feature[map_in[0]] < 1880:
            self.FES_ch_mode[map_out[1]] = 1
            logger2.debug("grasp")
        else:
            self.FES_ch_mode[map_out[1]] = 0

        # --- 神经网络动作识别 --- #
        # USE_NN_CLASSIFIER = False  # 设置为 True 以启用神经网络分类器
        # if USE_NN_CLASSIFIER:
        #     try:
                # 1. 准备模型输入 (示例)
                #    根据您的模型要求，可能需要对 RMS_feature进行预处理，
                #    例如：归一化、调整形状等。
                # model_input = preprocess_for_nn(RMS_feature, map_in)
                # logger2.debug("神经网络分类器：准备输入...")

                # 2. 执行模型推理 (概念性演示 - 替换为您的模型加载和推理代码)
                #    您需要加载您的 .onnx 模型 (或其他格式) 并执行推理。
                #    例如: 
                #    if not hasattr(self, 'ort_session'):
                #        self.ort_session = ort.InferenceSession("your_model.onnx")
                #    input_name = self.ort_session.get_inputs()[0].name
                #    ort_inputs = {input_name: model_input.astype(np.float32)}
                #    ort_outs = self.ort_session.run(None, ort_inputs)
                #    predicted_actions = ort_outs[0] # 假设输出是动作类别
                # logger2.debug("神经网络分类器：执行推理 (模拟结果)... ")
                # # 模拟推理结果 (0: 静息, 1: 屈腕, 2: 抓握)
                # # 这部分需要替换为真实的模型输出
                # simulated_nn_output_wrist = 0 # 假设模型预测屈腕为0 (静息)
                # simulated_nn_output_grasp = 0 # 假设模型预测抓握为0 (静息)
                # if RMS_feature[map_in[0]] > 1900: # 模拟屈腕检测
                #     simulated_nn_output_wrist = 1
                # if RMS_feature[map_in[1]] > 1900 and RMS_feature[map_in[0]] < 1880: # 模拟抓握检测
                #     simulated_nn_output_grasp = 2
                
                # predicted_action_wrist = simulated_nn_output_wrist
                # predicted_action_grasp = simulated_nn_output_grasp

                # # 3. 处理模型输出并更新 FES_ch_mode
                # logger2.debug(f"神经网络分类器输出：屈腕={predicted_action_wrist}, 抓握={predicted_action_grasp}")
                # if predicted_action_wrist == 1: # 假设 1 代表屈腕
                #     self.FES_ch_mode[map_out[0]] = 1
                #     logger2.debug("神经网络: WristFlexion")
                # else:
                #     self.FES_ch_mode[map_out[0]] = 0

                # if predicted_action_grasp == 2: # 假设 2 代表抓握
                #     self.FES_ch_mode[map_out[1]] = 1
                #     logger2.debug("神经网络: Grasp")
                # else:
                #     self.FES_ch_mode[map_out[1]] = 0
                
                # 如果神经网络分类成功，可以选择性地跳过下面的阈值判断
                # return # 直接返回，不执行后续的阈值判断

            # except Exception as e:
            #     logger2.error(f"神经网络分类器错误: {e}. 回退到阈值分类。")
            #     # 如果神经网络执行出错，则执行下面的原始阈值判断逻辑
            #     # (此处的阈值判断代码已在上方，所以这里不需要重复)
            #     pass # 确保即使出错，后续的原始逻辑也会执行（除非上面显式return）
        # --- 结束 --- #


    # todo: 非线性转换
    def non_liner_control(self) -> None:
        return

    def openloop_errot_elecontrol(self, mapin_ch=[0, 1], map_out=[2, 3]) -> None:
        # global data_queuerms
        pass

    def mapping_out(self, intensity_value: float):
        """
        将电刺激强度映射到0-10整数，对应前文电刺激强度映射表
        :param intensity_value: 原始电刺激强度值
        :return: 映射后的整数值(1-10)
        """
        # 设置映射范围
        min_intensity = 0  # 最小强度
        max_intensity = 100  # 最大强度
        max_output = 3
        # 限制输入值在有效范围内
        intensity_value = max(min_intensity, min(intensity_value, max_intensity))
        # 线性映射到0-10范围
        mapped_value = max_output * (intensity_value - min_intensity) / (max_intensity - min_intensity)
        # 四舍五入并转换为整数
        mapped_int = round(mapped_value)

        # 确保结果在1-10范围内
        return max(1, min(mapped_int, max_output))

    def num2FESconfig(self, num: int) -> FES_Config:
        """
        将整数映射到FES配置
        :param num: 整数
        :return: FES配置
        """
        if num == 0:
            return None
        else:
            return FES_config_map[num]

    def close_loop_bias_controller(self, rms_data, in_ch=0, out_ch=2) -> FES_Config:
        """
        基于前馈+PD控制器的对侧闭环控制系统线程
        计算in_ch和out_ch对应通道的rms差，使用pd控制器计算输出
        """
        # 计算误差
        diff = rms_data[in_ch] - rms_data[out_ch]

        # 计算PID控制器输出
        # 比例项
        p_term = self.Kp * diff

        # # 积分项 - 累积误差
        # self.integral += diff
        # i_term = self.Ki * self.integral

        # 微分项 - 误差变化率
        d_term = self.Kd * (diff - self.prev_error[out_ch])
        self.prev_error[out_ch] = diff

        # 前馈项
        ff_term = self.front_feed * (rms_data[in_ch] - FLE_Threshold)

        # 计算总控制输出
        control_output = p_term + d_term + ff_term

        # 将控制输出映射到FES强度
        intensity_value = max(0, min(10000, control_output))  # 限制在有效范围内

        # 映射到FES配置
        mapped_intensity = self.mapping_out(intensity_value)
        logger2.debug(f"ch:{in_ch} , pd输出: {control_output} ,FES强度配置: {mapped_intensity}")
        return self.num2FESconfig(mapped_intensity)

    def same_side_control(self, mapin_ch=[0, 1], map_out=[0, 1]):
        """
        同侧控制逻辑
        """
        pass

    def control_thread_loop(self):
        """
        电刺激控制线程
        主要功能:
        1. 从输入队列获取RMS数据
        2. 根据控制模式执行相应的控制逻辑
        3. 将FES配置发送到输出队列
        """

        print("FES control thread started.")
        while not self.stop_event.is_set():
            # todo
            try:
                print("FES control begin")
                current_rms_list_from_queue = self.rms_input_queue.get()
                if current_rms_list_from_queue is None:
                    print("RMS data is None (either not ready or failed to read). Waiting and retrying...")
                    # time.sleep(0.02)  # 等待数据
                    continue
                cur_time = time.time()
                if (cur_time - self.last_fes_time) < 1.2:
                    continue
                current_rms = current_rms_list_from_queue[0]
                # logger2.debug(f"current_rms_list_from_queue: {current_rms_list_from_queue}")
                # print(f"current_rms: f{current_rms}")

                logger2.debug(f"cur_fesmode is{self.control_mode}")
                # 检查控制模式
                if self.control_mode == 0:  # 手动控制
                    # 手动模式下，此线程不主动发送配置
                    pass
                elif self.control_mode == 1:  # 同侧控制
                    # 执行同侧控制逻辑
                    # todo: 实现同侧控制逻辑，根据 self.ch_RMS 计算 FES_Config
                    # 示例：假设通道0控制通道2，通道1控制通道3
                    map_in = [0, 1]
                    map_out = [0, 1]

                    # 示例逻辑：如果输入通道RMS超过阈值，则在对应输出通道生成配置
                    # 这里需要根据你的具体算法来计算 FES_Config 的参数
                    self.hand_classify(current_rms, map_in)  # 手动作分类
                    if self.FES_ch_mode[map_out[0]] == 1:  # 屈腕
                        for STI_intensity in [1, 3, 3, 2, 1]:
                            # 更新FES配置
                            tmp_fes_config = self.num2FESconfig(STI_intensity)
                            self.CH_fes_Config[map_out[0]].set_FES_config(tmp_fes_config)
                            # 发送FES配置到输出队列
                            self.fes_output_queue.put(self.CH_fes_Config[map_out[0]])

                    # 注意：这里需要添加逻辑来停止刺激，例如当RMS低于某个阈值时发送强度为0的配置，或者发送停止指令

                elif self.control_mode == 2:  # 对侧闭环控制
                    # 执行对侧闭环控制逻辑 (带投票机制)
                    map_in = [0, 1]
                    map_out = [2, 3]

                    self.hand_classify(current_rms, map_in, map_out)  # 手动作分类
                    current_intensity = None
                    # 屈腕判定成功
                    if self.FES_ch_mode[map_out[0]] == 1:  # 屈腕
                        print(f"current_rms: f{current_rms}")
                        current_intensity = 1  # 默认强度
                        if 1930 < current_rms[map_in[0]] < 2020:
                            current_intensity = 2
                        elif 2020 <= current_rms[map_in[0]] < 2100:
                            current_intensity = 3
                        self.intensity_history1.append(current_intensity)
                        logger2.info(f"动作录入{current_intensity}， deque_len:{len(self.intensity_history1)}")
                    # 未能屈腕，投票结束
                    else:
                        logger2.info("腕屈动作结束")
                        self.intensity_history1.clear()

                        # 抓握判定成功
                        if self.FES_ch_mode[map_out[1]] == 1:  # 抓握
                            print(f"current_rms: f{current_rms}")
                            current_intensity = 1  # 默认强度
                            if 1880 < current_rms[map_in[0]] < 1980:
                                current_intensity = 2
                            elif 1980 <= current_rms[map_in[0]] < 2100:
                                current_intensity = 3
                            self.intensity_history2.append(current_intensity)
                            logger2.info(f"动作抓握录入{current_intensity}， deque_len:{len(self.intensity_history2)}")
                        # 未能屈腕，投票结束
                        else:
                            logger2.info("抓握动作结束")
                            self.intensity_history1.clear()

                    if len(self.intensity_history1) == self.VOTING_WINDOW_SIZE:  # 确保配置不是 None
                        current_intensity = max(self.intensity_history1)
                        tmp_fes_config = self.num2FESconfig(current_intensity)
                        self.CH_fes_Config[map_out[0]].set_FES_config(tmp_fes_config)
                        self.intensity_history1.clear()  # 清空历史记录
                        logger2.info(f"ch:{map_out[0]} , Voted Intensity: {current_intensity}")
                        self.fes_output_queue.put(self.CH_fes_Config[map_out[0]])
                        # time.sleep(0.1)  # 短暂延时
                        self.last_fes_time = time.time()
                    if len(self.intensity_history2) == self.VOTING_WINDOW_SIZE:  # 确保配置不是 None
                        current_intensity = max(self.intensity_history2)
                        tmp_fes_config = self.num2FESconfig(current_intensity)
                        self.CH_fes_Config[map_out[1]].set_FES_config(tmp_fes_config)
                        self.intensity_history2.clear()  # 清空历史记录
                        logger2.info(f"ch:{map_out[1]} , Voted Intensity: {current_intensity}")
                        self.fes_output_queue.put(self.CH_fes_Config[map_out[0]])
                        # time.sleep(0.1)  # 短暂延时
                        self.last_fes_time = time.time()


            except queue.Empty:
                # 队列为空，继续循环等待
                time.sleep(0.01)  # 发生错误时稍作等待
                continue
            except Exception as e:
                print(f"Error in FES control thread: {e}")
                time.sleep(0.1)  # 发生错误时稍作等待

            # 控制循环频率
            time.sleep(0.02)  # 可选，根据需要调整处理频率
        print("FES control thread stopped.")

    def stop(self):  # 添加停止方法
        """Signals the control thread to stop."""
        self.stop_event.set()


# def _detect_grasp_begin(rms_data: int) -> bool:
#     """检测抓握开始（压力阈值）"""
#     return np.mean(rms_data) >= 0.5  # 压力阈值示例


# def _detect_wrist_begin(rms_data: int) -> bool:
#     """检测屈腕开始（角度阈值）"""
#     return np.mean(rms_data) >= 0.5  # 压力阈值示例


# def _detect_wrist_success(emg_data: int) -> bool:
#     return True


# class StateMachineCoSideController:
#     """
#     同侧闭环控制有限状态机控制器
#     状态管理：根据传感器反馈和用户意图切换电刺激模式
#     支持状态：
#     0 - 静息状态
#     1 - 抓握控制
#     2 - 屈腕控制
#     """

#     # 状态定义（修正为需求的3个状态）
#     class State:
#         REST = 0
#         GRASP = 1
#         WRIST_FLEX = 2

#     # 新增状态配置映射表
#     STATE_CONFIG = {
#         State.REST: {
#             'worktime': 0,
#             'lastingtime': 0,
#             'holdtime': 0,
#             'intensity': 0
#         },
#         State.GRASP: {
#             'worktime': 1800,
#             'lastingtime': 120,
#             'holdtime': 60,
#             'intensity': 1000
#         },
#         State.WRIST_FLEX: {
#             'worktime': 2000,
#             'lastingtime': 150,
#             'holdtime': 50,
#             'intensity': 900
#         }
#     }

#     def __init__(self, fes_controller: FES_Controler) -> None:
#         self.fes = fes_controller
#         self.current_state = self.State.REST
#         self.transition_timeout = 0
#         self.MAX_TIMEOUT = 500  # 超时阈值（单位：ms）
#         self.last_angle = 0  # 记录角度变化
#         self.target_angle = 90  # 目标角度（示例）

#     def transition_to(self, new_state: int, sensor_data: dict) -> None:
#         if new_state == self.State.REST:
#             self.fes.hand_WristFlexion = 0
#             self.fes.hand_Grasp = 0

#     def process_sensor_feedback(self, emg_data: np.ndarray) -> None:
#         """
#         根据传感器数据处理状态转换
#         :param emg_data: 实时sEMG信号特征
#         :param sensor_data: 肌肉运动反馈数据（角度/压力等）
#         """
#         # 获取当前控制意图（通过EMG分类）
#         # intent = self.fes.hand_classify(emg_data)

#         # 状态转换逻辑
#         if self.current_state == self.State.REST:
#             if _detect_grasp_begin(emg_data):  # 检测到抓握意图（四指屈）
#                 self.transition_to(self.State.GRASP, emg_data)
#         elif self.current_state == self.State.GRASP:
#             if _detect_wrist_begin(emg_data):  # 抓握完成
#                 self.transition_to(self.State.WRIST_FLEX, emg_data)
#             elif self._detect_timeout():  # 超时返回静息
#                 self.transition_to(self.State.REST, emg_data)
#         elif self.current_state == self.State.WRIST_FLEX:
#             if self._detect_wrist_success(emg_data):  # 屈腕完成
#                 self.transition_to(self.State.REST, emg_data)
#             elif self._detect_timeout():  # 超时返回静息
#                 self.transition_to(self.State.REST, emg_data)

#     def _detect_timeout(self) -> bool:
#         """超时检测（500ms未完成动作）"""
#         return self.transition_timeout >= self.MAX_TIMEOUT

#     def _enter_new_state(self, sensor_data: dict) -> None:
#         """状态进入时的配置更新"""
#         if self.current_state == self.State.GRASP:
#             self._configure_grasp(sensor_data)
#         elif self.current_state == self.State.WRIST_FLEX:
#             self._configure_wrist_flex(sensor_data)

#     def _configure_grasp(self, sensor_data: dict) -> None:
#         """抓握控制配置"""
#         # 确保索引在有效范围内
#         grasp_index = min(self.State.GRASP, len(self.fes.CH_fes_Config) - 1)
#         config = self.fes.CH_fes_Config[grasp_index]
#         config.set_FESWave_Form(300, 2)  # 波形参数
#         config.set_FES_config(1800, 120, 60)  # 预设参数
#         pressure = sensor_data.get('pressure', 0)
#         config.ele_Intensity = min(1000, 500 + pressure * 2)  # 压力自适应

#     def _configure_wrist_flex(self, sensor_data: dict) -> None:
#         """屈腕控制配置"""
#         # 确保索引在有效范围内
#         wrist_flex_index = min(self.State.WRIST_FLEX, len(self.fes.CH_fes_Config) - 1)
#         config = self.fes.CH_fes_Config[wrist_flex_index]
#         config.set_FES_config(2000, 150, 50)  # 预设参数
#         config.ele_Intensity = 900  # 初始电压
#         # todo: 设置曲线

#     def _exit_current_state(self) -> None:
#         """状态退出时的清理操作"""
#         if self.current_state != self.State.REST:
#             # 保存当前配置作为历史记录
#             # 确保索引在有效范围内
#             current_index = min(self.current_state, len(self.fes.CH_fes_Config) - 1)
#             self.fes.CH_fes_Config[current_index].save_config()
#             # 重置相关参数
#             self.fes.hand_Grasp = 0
#             self.fes.hand_WristFlexion = 0


if __name__ == '__main__':
    fes_config = FES_Config()
    """
    调试相关计算函数
    """
    print(fes_config.overrall_intensity)
    print(FES_Controler.calculate_Equivalent_intensity(fes_config.overrall_intensity))
